/*
 * SPDX-FileCopyrightText: 2021-2023 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Unlicense OR CC0-1.0
 */
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#include "esp_bit_defs.h"
#include "esp_spp_api.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "hal/ledc_types.h"
#include "idf_additions.h"
#include "led_strip_types.h"
#include "nvs_flash.h"
#include "esp_bt.h"

#if CONFIG_BT_NIMBLE_ENABLED
#include "host/ble_hs.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#else
#include "esp_bt_defs.h"
#include "esp_gap_ble_api.h"
#include "esp_gatts_api.h"
#include "esp_gatt_defs.h"
#include "esp_bt_main.h"
#include "esp_bt_device.h"
#endif

#if CONFIG_BT_NIMBLE_ENABLED
#include "host/ble_hs.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#define ESP_BD_ADDR_STR         "%02x:%02x:%02x:%02x:%02x:%02x"
#define ESP_BD_ADDR_HEX(addr)   addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]
#else
#include "esp_bt_defs.h"
#include "esp_gap_ble_api.h"
#include "esp_gatts_api.h"
#include "esp_gatt_defs.h"
#include "esp_bt_main.h"
#include "esp_bt_device.h"
#endif

#include "esp_hidh.h"
#include "esp_hid_gap.h"
#include "driver/ledc.h"
#include "freertos/semphr.h"
#include "led_strip.h"
#include "math.h"

#define PI                  3.1415926535897932384626433832795

#define LEDC_FREQ_HZ        1000
#define LEDC_DUTY_RES       LEDC_TIMER_15_BIT
#define LEDC_DUTY_DEFAULT   (32767)

#define LEDC_LEFT_PIN       (5)
#define LEDC_RIGHT_PIN      (25)

#define LEDC_LEFT_CHANNEL   LEDC_CHANNEL_0
#define LEDC_RIGHT_CHANNEL  LEDC_CHANNEL_1
#define LEDC_FADE_TIME      (100)
#define LEDC_POWER_LEVEL_MAX (0.79f)
#define LEDC_POWER_LEVEL_MIM (0.21f)

#define LED_STRIP_BLINK_GPIO (21)
#define LED_STRIP_LED_NUMBERS (1)
#define LED_STRIP_RMT_RES_HZ  (10 * 1000 * 1000)

#define GAME_HANDLE_SHOULDER_LB         (1 << 0)    // L1
#define GAME_HANDLE_SHOULDER_RB         (1 << 1)    // R1
#define GAME_HANDLE_SHOULDER_LT         (1 << 2)    // L2
#define GAME_HANDLE_SHOULDER_RT         (1 << 3)    // R2
#define GAME_HANDLE_SHOULDER_SHARE      (1 << 4)
#define GAME_HANDLE_SHOULDER_OPTIONS    (1 << 5)
#define GAME_HANDLE_SHOULDER_LS         (1 << 6)    // L3
#define GAME_HANDLE_SHOULDER_RS         (1 << 7)    // R3

#define GAME_HANDLE_KEY_UP      (1 << 3)   // 位与取反
#define GAME_HANDLE_KEY_DOWN    (1 << 2)
#define GAME_HANDLE_KEY_LEFT    (3 << 1)
#define GAME_HANDLE_KEY_RIGHT   (1 << 1)
#define GAME_HANDLE_KEY_A       (1 << 5)   // 叉叉
#define GAME_HANDLE_KEY_B       (1 << 6)   // 圆圈
#define GAME_HANDLE_KEY_X       (1 << 4)   // 方块
#define GAME_HANDLE_KEY_Y       (1 << 7)   // 三角

static const char *TAG = "ROV";

// 定义卡尔曼滤波器结构体
typedef struct {
    float q; // 过程噪声协方差
    float r; // 测量噪声协方差
    float x; // 值的估计
    float p; // 估计协方差
    float k; // 卡尔曼增益
} kalman_filter_t;

struct ROV {
    struct {
        esp_hidh_dev_t *dev;
        union{
            uint8_t data[9];
            struct {
                uint8_t lx, ly, rx, ry, key, shoulder, unknown, lt, rt;
            } item;
        } report;
    } controller;
    struct {
        float level;
        uint8_t power;
        uint8_t direction;
        uint16_t left, right;
    } engine;
    struct {
        SemaphoreHandle_t left, right;
    } semaphore;
    struct {
        led_strip_handle_t handle;
        uint8_t blink;
    } led;
} rov = {
    .engine = {
        .level = 0.2f,
        .power = 0,
        .direction = 0x80,
        .left = 0,
        .right = 0
    },
    .semaphore = {0},
    .led = {0}
};

kalman_filter_t kf_power = {
    .q = 0.008f,
    .r = 0.012f,
    .x = 0.0f,
    .p = 1.0f,
    .k = 0.0f
};
kalman_filter_t kf_direction = {
    .q = 0.008f,
    .r = 0.012f,
    .x = 0.0f,
    .p = 1.0f,
    .k = 0.0f
};

TaskHandle_t ledc_left_handle = NULL;
TaskHandle_t ledc_right_handle = NULL;


void hid_link_task(void *pvParameters);

led_strip_handle_t led_strip_init()
{
    // LED strip general initialization, according to your led board design
    led_strip_config_t strip_config = {
        .strip_gpio_num = LED_STRIP_BLINK_GPIO,   // The GPIO that connected to the LED strip's data line
        .max_leds = LED_STRIP_LED_NUMBERS,        // The number of LEDs in the strip,
        .led_pixel_format = LED_PIXEL_FORMAT_GRB, // Pixel format of your LED strip
        .led_model = LED_MODEL_WS2812,            // LED strip model
        .flags.invert_out = false,                // whether to invert the output signal
    };

    // LED strip backend configuration: RMT
    led_strip_rmt_config_t rmt_config = {
#if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0)
        .rmt_channel = 0,
#else
        .clk_src = RMT_CLK_SRC_DEFAULT,        // different clock source can lead to different power consumption
        .resolution_hz = LED_STRIP_RMT_RES_HZ, // RMT counter clock frequency
        .flags.with_dma = false,               // DMA feature is available on ESP target like ESP32-S3
#endif
    };

    // LED Strip object handle
    led_strip_handle_t led_strip;
    ESP_ERROR_CHECK(led_strip_new_rmt_device(&strip_config, &rmt_config, &led_strip));
    ESP_LOGI(TAG, "Created LED strip object with RMT backend");
    return led_strip;
}

static IRAM_ATTR bool ledc_fade_event_cb(const ledc_cb_param_t *param, void *user_arg)
{
    portBASE_TYPE taskAwoken = pdFALSE;

    if (param->event == LEDC_FADE_END_EVT) {
        SemaphoreHandle_t counting_sem = (SemaphoreHandle_t) user_arg;
        xSemaphoreGiveFromISR(counting_sem, &taskAwoken);
    }

    return (taskAwoken == pdTRUE);
}

void recursive_mean_filter(uint8_t data)
{
#define FILTER_SIZE 10
#define MEAN_DIFFERENCE 5

    uint16_t sum = 0;
    uint8_t avg = 0;
    static uint8_t index = 0;
    static uint8_t array[FILTER_SIZE] = {0};

    array[index] = data;
    for (uint8_t i = 0; i < FILTER_SIZE; i++) {
        sum += array[i];
    }
    if (index < FILTER_SIZE)
        ++index;
    else
        index = 0;
    avg = sum / FILTER_SIZE;
    if (abs(avg - data) > MEAN_DIFFERENCE) {
        rov.engine.power = avg;
    } else {
        rov.engine.power = data;
    }
}

// 卡尔曼滤波更新
uint8_t kalman_filter_update(kalman_filter_t* kf, uint8_t measurement) {
    // 预测更新
    kf->p = kf->p + kf->q;
    // 计算卡尔曼增益
    kf->k = kf->p / (kf->p + kf->r);
    // 更新估计值
    kf->x = kf->x + kf->k * (measurement - kf->x);
    // 更新估计协方差
    kf->p = (1 - kf->k) * kf->p;
    // 返回滤波后的值，确保类型转换
    return (uint8_t)kf->x;
}

void engine_power_calculate()
{
    static float coefficient_left = 0, coefficient_right = 0;

    if (rov.engine.direction > 0x80) {
        coefficient_right = ((float)rov.engine.power / 0xff) * ((float)(0xff - rov.engine.direction) / 0x80);
        coefficient_left = (float)rov.engine.power / 0xff;
    } else if (rov.engine.direction < 0x80) {
        coefficient_left = ((float)rov.engine.power / 0xff) * ((float)rov.engine.direction / 0x80);
        coefficient_right = (float)rov.engine.power / 0xff;
    } else {
        coefficient_right = (float)rov.engine.power / 0xff;
        coefficient_left = (float)rov.engine.power / 0xff;
    }
    rov.engine.right = LEDC_DUTY_DEFAULT * rov.engine.level * coefficient_right;
    rov.engine.left = LEDC_DUTY_DEFAULT * rov.engine.level * coefficient_left;
}

uint8_t beatsin8(uint8_t beats_per_minute, uint8_t low_value, uint8_t high_value) {
    uint64_t subtle = esp_timer_get_time();
    double seconds = subtle / 1000000.0;
    double period = 60.0 / beats_per_minute;  // period in seconds
    double time_in_period = fmod(seconds, period) / period;
    double sine_value = sin(time_in_period * 2 * PI);  // Sine wave from -1 to 1
    uint8_t result = (uint8_t) ((sine_value + 1) * 0.5 * (high_value - low_value) + low_value);
    return result;
}

void led_refresh_task(void *pvParameters)
{
    led_strip_handle_t led_strip = (led_strip_handle_t)pvParameters;
    uint16_t hue = 0;
    uint8_t saturation = 255;
    uint8_t value = 0;

    for (;;) {
        if (rov.led.blink == 0)
        {
            hue = 320;
            value = beatsin8(30, 0, 96);
        } else if (rov.led.blink == 1) {
            if ((rov.controller.report.item.shoulder & GAME_HANDLE_SHOULDER_LT) && rov.engine.power)
            {
                ++hue;
                if (hue > 359) {
                    hue = 0;
                }
            }
            value = rov.engine.power * rov.engine.level;
        }
        led_strip_set_pixel_hsv(led_strip, 0, hue, saturation, value);
        led_strip_refresh(led_strip);
        vTaskDelay(15 / portTICK_PERIOD_MS);
    }
}

void ledc_right_ctrl_task(void *pvParameters)
{
    uint16_t duty_last = 0;
    xSemaphoreGive(rov.semaphore.right);
    for (;;) {
        xSemaphoreTake(rov.semaphore.right, portMAX_DELAY);
        while (rov.engine.right == duty_last)
            vTaskDelay(1);
        ledc_set_fade_with_time(LEDC_HIGH_SPEED_MODE,
                                LEDC_RIGHT_CHANNEL,
                                rov.engine.right,
                                LEDC_FADE_TIME * ((float)abs(duty_last - rov.engine.right) / (LEDC_DUTY_DEFAULT * rov.engine.level)));
        ledc_fade_start(LEDC_HIGH_SPEED_MODE, LEDC_RIGHT_CHANNEL, LEDC_FADE_NO_WAIT);
        duty_last = rov.engine.right;
    }
}

void ledc_left_ctrl_task(void *pvParameters)
{
    uint16_t duty_last = 0;
    xSemaphoreGive(rov.semaphore.left);
    for (;;) {
        xSemaphoreTake(rov.semaphore.left, portMAX_DELAY);
        while (rov.engine.left == duty_last)
            vTaskDelay(1);
        ledc_set_fade_with_time(LEDC_HIGH_SPEED_MODE,
                                LEDC_LEFT_CHANNEL,
                                rov.engine.left,
                                LEDC_FADE_TIME * ((float)abs(duty_last - rov.engine.left) / (LEDC_DUTY_DEFAULT * rov.engine.level)));
        ledc_fade_start(LEDC_HIGH_SPEED_MODE, LEDC_LEFT_CHANNEL, LEDC_FADE_NO_WAIT);
        duty_last = rov.engine.left;
    }
}

void hidh_callback(void *handler_args, esp_event_base_t base, int32_t id, void *event_data)
{
    static uint8_t key_up = 1, key_down = 1;
    esp_hidh_event_t event = (esp_hidh_event_t)id;
    esp_hidh_event_data_t *param = (esp_hidh_event_data_t *)event_data;

    // static uint8_t report_id = 1;
    // static uint8_t cmd[32] = {0x52, 0x11, 0x80, 0x00, 0xff, 0x00, 0x00, 0x40, 0x40};

    switch (event) {
    case ESP_HIDH_OPEN_EVENT: {
        if (param->open.status == ESP_OK) {
            const uint8_t *bda = esp_hidh_dev_bda_get(param->open.dev);
            ESP_LOGI(TAG, ESP_BD_ADDR_STR " OPEN: %s", ESP_BD_ADDR_HEX(bda), esp_hidh_dev_name_get(param->open.dev));
            esp_hidh_dev_dump(param->open.dev, stdout);

            rov.controller.dev = param->open.dev;
            rov.led.blink = 1;
            if(ledc_left_handle == NULL && ledc_right_handle == NULL) {
                xTaskCreate(ledc_left_ctrl_task, "ledc_left", 4 * 1024, NULL, 2, &ledc_left_handle);
                xTaskCreate(ledc_right_ctrl_task, "ledc_right", 4 * 1024, NULL, 2, &ledc_right_handle);
            }
        } else {
            ESP_LOGE(TAG, " OPEN failed!");
        }
        break;
    }
    case ESP_HIDH_BATTERY_EVENT: {
        const uint8_t *bda = esp_hidh_dev_bda_get(param->battery.dev);
        ESP_LOGI(TAG, ESP_BD_ADDR_STR " BATTERY: %d%%", ESP_BD_ADDR_HEX(bda), param->battery.level);
        break;
    }
    case ESP_HIDH_INPUT_EVENT: {
        // const uint8_t *bda = esp_hidh_dev_bda_get(param->input.dev);
        // ESP_LOGI(TAG, ESP_BD_ADDR_STR " INPUT: %8s, MAP: %2u, ID: %3u, Len: %d, Data:", ESP_BD_ADDR_HEX(bda), esp_hid_usage_str(param->input.usage), param->input.map_index, param->input.report_id, param->input.length);
        // ESP_LOG_BUFFER_HEX(TAG, param->input.data, param->input.length);
        memcpy(rov.controller.report.data, param->input.data, param->input.length);
        rov.engine.power = kalman_filter_update(&kf_power ,rov.controller.report.item.rt);
        rov.engine.direction = kalman_filter_update(&kf_direction, rov.controller.report.item.lx);
        if(key_up)
        {
            if (rov.controller.report.item.key & GAME_HANDLE_KEY_Y) {
                // esp_hidh_dev_output_set(param->open.dev, 0, report_id++, cmd, sizeof(cmd));
                if(rov.engine.level < LEDC_POWER_LEVEL_MAX)
                    rov.engine.level += 0.2f;
            }
            key_up = 0;
        }
        if (key_down)
        {
            if ((rov.controller.report.item.key & GAME_HANDLE_KEY_A) && (rov.engine.level > LEDC_POWER_LEVEL_MIM))
                rov.engine.level -= 0.2f;
            key_down = 0;
        }
        if(!(rov.controller.report.item.key & GAME_HANDLE_KEY_Y))
            key_up = 1;
        if(!(rov.controller.report.item.key & GAME_HANDLE_KEY_A))
            key_down = 1;
        engine_power_calculate();
        break;
    }
    case ESP_HIDH_FEATURE_EVENT: {
        const uint8_t *bda = esp_hidh_dev_bda_get(param->feature.dev);
        ESP_LOGI(TAG, ESP_BD_ADDR_STR " FEATURE: %8s, MAP: %2u, ID: %3u, Len: %d", ESP_BD_ADDR_HEX(bda),
                 esp_hid_usage_str(param->feature.usage), param->feature.map_index, param->feature.report_id,
                 param->feature.length);
        ESP_LOG_BUFFER_HEX(TAG, param->feature.data, param->feature.length);
        break;
    }
    case ESP_HIDH_CLOSE_EVENT: {
        const uint8_t *bda = esp_hidh_dev_bda_get(param->close.dev);
        rov.led.blink = 0;
        xTaskCreate(&hid_link_task, "hid_task", 6 * 1024, NULL, 2, NULL);
        ESP_LOGI(TAG, ESP_BD_ADDR_STR " CLOSE: %s", ESP_BD_ADDR_HEX(bda), esp_hidh_dev_name_get(param->close.dev));
        rov.engine.power = 0;
        rov.engine.direction = 0x80;
        vTaskDelete(ledc_left_handle);
        vTaskDelete(ledc_right_handle);
        ledc_left_handle = NULL;
        ledc_right_handle = NULL;
        break;
    }
    default:
        ESP_LOGI(TAG, "EVENT: %d", event);
        break;
    }
}

#define SCAN_DURATION_SECONDS 5

void hid_link_task(void *pvParameters)
{
    size_t results_len = 0;
    esp_hid_scan_result_t *results = NULL;
SCAN:
    rov.led.blink = 0;
    ESP_LOGI(TAG, "SCAN...");
    //start scan for HID devices
    esp_hid_scan(SCAN_DURATION_SECONDS, &results_len, &results);
    ESP_LOGI(TAG, "SCAN: %zu results", results_len);
    if (results_len) {
        esp_hid_scan_result_t *r = results;
        esp_hid_scan_result_t *cr = NULL;
        while (r) {
            printf("  %s: " ESP_BD_ADDR_STR ", ", (r->transport == ESP_HID_TRANSPORT_BLE) ? "BLE" : "BT ", ESP_BD_ADDR_HEX(r->bda));
            printf("RSSI: %d, ", r->rssi);
            printf("USAGE: %s, ", esp_hid_usage_str(r->usage));
#if CONFIG_BT_BLE_ENABLED
            if (r->transport == ESP_HID_TRANSPORT_BLE) {
                cr = r;
                printf("APPEARANCE: 0x%04x, ", r->ble.appearance);
                printf("ADDR_TYPE: '%s', ", ble_addr_type_str(r->ble.addr_type));
            }
#endif /* CONFIG_BT_BLE_ENABLED */
#if CONFIG_BT_NIMBLE_ENABLED
            if (r->transport == ESP_HID_TRANSPORT_BLE) {
                cr = r;
                printf("APPEARANCE: 0x%04x, ", r->ble.appearance);
                printf("ADDR_TYPE: '%d', ", r->ble.addr_type);
            }
#endif /* CONFIG_BT_BLE_ENABLED */
#if CONFIG_BT_HID_HOST_ENABLED
            if (r->transport == ESP_HID_TRANSPORT_BT) {
                cr = r;
                printf("COD: %s[", esp_hid_cod_major_str(r->bt.cod.major));
                esp_hid_cod_minor_print(r->bt.cod.minor, stdout);
                printf("] srv 0x%03x, ", r->bt.cod.service);
                print_uuid(&r->bt.uuid);
                printf(", ");
            }
#endif /* CONFIG_BT_HID_HOST_ENABLED */
            printf("NAME: %s ", r->name ? r->name : "");
            printf("\n");
            r = r->next;
        }
        if (cr) {
            //open the last result
            esp_hidh_dev_open(cr->bda, cr->transport, cr->ble.addr_type);
        }
        //free the results
        esp_hid_scan_results_free(results);
    } else {
        goto SCAN;
    }
    vTaskDelete(NULL);
}

#if CONFIG_BT_NIMBLE_ENABLED
void ble_hid_host_task(void *param)
{
    ESP_LOGI(TAG, "BLE Host Task Started");
    /* This function will return only when nimble_port_stop() is executed */
    nimble_port_run();

    nimble_port_freertos_deinit();
}
void ble_store_config_init(void);
#endif
void app_main(void)
{
    esp_err_t ret;
#if HID_HOST_MODE == HIDH_IDLE_MODE
    ESP_LOGE(TAG, "Please turn on BT HID host or BLE!");
    return;
#endif
    ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK( ret );

    rov.led.handle = led_strip_init();
    led_strip_clear(rov.led.handle);
    xTaskCreate(led_refresh_task, "led", 1024 * 2, rov.led.handle, 2, NULL);

    ESP_LOGI(TAG, "setting hid gap, mode:%d", HID_HOST_MODE);
    ESP_ERROR_CHECK( esp_hid_gap_init(HID_HOST_MODE) );
#if CONFIG_BT_BLE_ENABLED
    ESP_ERROR_CHECK( esp_ble_gattc_register_callback(esp_hidh_gattc_event_handler) );
#endif /* CONFIG_BT_BLE_ENABLED */
    esp_hidh_config_t config = {
        .callback = hidh_callback,
        .event_stack_size = 4096,
        .callback_arg = NULL,
    };
    ESP_ERROR_CHECK( esp_hidh_init(&config) );

#if CONFIG_BT_NIMBLE_ENABLED
    /* XXX Need to have template for store */
    ble_store_config_init();

    ble_hs_cfg.store_status_cb = ble_store_util_status_rr;
	/* Starting nimble task after gatts is initialized*/
    ret = esp_nimble_enable(ble_hid_host_task);
    if (ret) {
        ESP_LOGE(TAG, "esp_nimble_enable failed: %d", ret);
    }
#endif
    xTaskCreate(&hid_link_task, "hid_task", 6 * 1024, NULL, 3, NULL);

    /* LEDC init */

    ledc_timer_config_t ledc_timer = {
        .duty_resolution = LEDC_DUTY_RES,       // resolution of PWM duty
        .freq_hz = LEDC_FREQ_HZ,                // frequency of PWM signal
        .speed_mode = LEDC_HIGH_SPEED_MODE,     // timer mode
        .timer_num = LEDC_TIMER_0,              // timer index
        .clk_cfg = LEDC_AUTO_CLK,
    };
    ledc_timer_config(&ledc_timer);
    ledc_channel_config_t ledc_channel = {
        .channel    = LEDC_LEFT_CHANNEL,
        .duty       = 0,
        .gpio_num   = LEDC_LEFT_PIN,
        .speed_mode = LEDC_HIGH_SPEED_MODE,
        .hpoint     = 0,
        .timer_sel  = LEDC_TIMER_0,
        .flags.output_invert = 1
    };
    ledc_channel_config(&ledc_channel); // left
    ledc_channel.channel = LEDC_RIGHT_CHANNEL;
    ledc_channel.gpio_num = LEDC_RIGHT_PIN;
    ledc_channel_config(&ledc_channel); // right

    ledc_fade_func_install(0);
    ledc_cbs_t callback = {
        .fade_cb = ledc_fade_event_cb
    };
    rov.semaphore.left = xSemaphoreCreateBinary();
    rov.semaphore.right = xSemaphoreCreateBinary();
    ledc_cb_register(LEDC_HIGH_SPEED_MODE, LEDC_LEFT_CHANNEL, &callback, rov.semaphore.left);
    ledc_cb_register(LEDC_HIGH_SPEED_MODE, LEDC_RIGHT_CHANNEL, &callback, rov.semaphore.right);
}
